import cv2
import mediapipe as mp
from pinpong.board import Board,Pin,NeoPixel
from pinpong.libs.microbit_motor import DFServo
import time
cv2.namedWindow("w", cv2.WND_PROP_FULLSCREEN)
cv2.setWindowProperty("w", cv2.WND_PROP_FULLSCREEN, cv2.WINDOW_FULLSCREEN)
img = cv2.imread("logo.jpg", cv2.IMREAD_UNCHANGED)
cv2.imshow("w", img)
if cv2.waitKey(1) & 0xFF == 27:  # 按ESC退出
        pass
# 主程序开始
Board().begin()
# 初始化灯带
pin1 = Pin(Pin.D13)
np1 = NeoPixel(pin1,7)
np1.brightness(128)
np1.brightness(255)
np1.clear()
# 初始化5个舵机，分别对应大拇指到小指
p_dfr0548_servo_S1 = DFServo(4)  # 大拇指
p_dfr0548_servo_S2 = DFServo(5)  # 食指
p_dfr0548_servo_S3 = DFServo(6)  # 中指
p_dfr0548_servo_S4 = DFServo(7)  # 无名指
p_dfr0548_servo_S5 = DFServo(8)  # 小指

# 初始化舵机位置（弯曲状态，120度）
p_dfr0548_servo_S1.angle(120)
p_dfr0548_servo_S2.angle(120)
p_dfr0548_servo_S3.angle(120)
p_dfr0548_servo_S4.angle(120)
p_dfr0548_servo_S5.angle(120)

# 初始化MediaPipe手部模型
mp_hands = mp.solutions.hands
mp_drawing = mp.solutions.drawing_utils
hands = mp_hands.Hands(static_image_mode=False,
                       max_num_hands=1,
                       min_detection_confidence=0.5,
                       min_tracking_confidence=0.5)

# 打开摄像头
cap = cv2.VideoCapture(0)
cap.set(cv2.CAP_PROP_FRAME_WIDTH, 240)  #设置摄像头图像宽度
cap.set(cv2.CAP_PROP_FRAME_HEIGHT, 320) #设置摄像头图像高度
cap.set(cv2.CAP_PROP_BUFFERSIZE, 1)     #设置OpenCV内部的图像缓存，可以极大提高图像的实时性。
# 存储上一帧的手指状态，用于检测变化
previous_finger_state = [False, False, False, False, False]  # 拇指,食指,中指,无名指,小指
finger_names = ['Thumb', 'Index', 'Middle', 'Ring', 'Pinky']
time.sleep(5)
def control_servos(current_state):
    """根据手指状态控制舵机"""
    # 大拇指控制
    if current_state[0]:  # 伸直
        p_dfr0548_servo_S1.angle(45)
        np1[0] = (0,0,0)
        
    else:  # 弯曲
        np1[0] = (255,0,0)
        p_dfr0548_servo_S1.angle(178)

    # 食指控制
    if current_state[1]:  # 伸直
        p_dfr0548_servo_S2.angle(178)
        np1[1] = (0,0,0)
    else:  # 弯曲
        np1[1] = (255,0,0)
        p_dfr0548_servo_S2.angle(45)
    
    # 中指控制
    if current_state[2]:  # 伸直
        p_dfr0548_servo_S3.angle(45)
        np1[2] = (0,0,0)
    else:  # 弯曲
        np1[2] = (255,0,0)
        p_dfr0548_servo_S3.angle(178)
    
    # 无名指控制
    if current_state[3]:  # 伸直
        p_dfr0548_servo_S4.angle(45)
        np1[3] = (0,0,0)
    else:  # 弯曲
        np1[3] = (255,0,0)
        p_dfr0548_servo_S4.angle(178)
    
    # 小指控制
    if current_state[4]:  # 伸直
        p_dfr0548_servo_S5.angle(45)
        np1[4] = (0,0,0)
    else:  # 弯曲
        np1[4] = (255,0,0)
        p_dfr0548_servo_S5.angle(178)
    
    # 打印当前状态
    status_code = ''.join(['1' if state else '0' for state in current_state])
    extended_fingers = [finger_names[i] for i, state in enumerate(current_state) if state]
    status_description = f"Extended: {', '.join(extended_fingers)}" if extended_fingers else "All fingers bent"
    
    print(f"Finger status: {status_code} - {status_description}")

while True:
    ret, frame = cap.read()
    if not ret:
        break
        
    # 水平翻转帧，使体验更直观
    frame = cv2.flip(frame, 1)
    # MediaPipe需要RGB格式的图像
    rgb_frame = cv2.cvtColor(frame, cv2.COLOR_BGR2RGB)
    
    # 处理帧，检测手部
    results = hands.process(rgb_frame)
    
    # 当前帧的手指状态
    current_finger_state = [False, False, False, False, False]
    finger_count = 0
    
    if results.multi_hand_landmarks:
        for hand_landmarks in results.multi_hand_landmarks:
            # 绘制手部关键点和连接线
            mp_drawing.draw_landmarks(frame, hand_landmarks, mp_hands.HAND_CONNECTIONS)
            
            # 获取手腕关键点作为参考点
            wrist_x = hand_landmarks.landmark[0].x
            wrist_y = hand_landmarks.landmark[0].y
            
            # 定义指尖关键点和其对应的第二关节关键点
            finger_tips = [4, 8, 12, 16, 20]  # 拇指,食指,中指,无名指,小指
            finger_pips = [2, 6, 10, 14, 18]  # 第二关节
            
            for i, (tip, pip) in enumerate(zip(finger_tips, finger_pips)):
                tip_x = hand_landmarks.landmark[tip].x
                tip_y = hand_landmarks.landmark[tip].y
                pip_x = hand_landmarks.landmark[pip].x
                pip_y = hand_landmarks.landmark[pip].y
                
                # 判断手指是否伸直
                if i == 0:  # 拇指
                    # 根据手的左右判断拇指伸直条件
                    is_extended = (tip_x < pip_x) if hand_landmarks.landmark[5].x < wrist_x else (tip_x > pip_x)
                else:  # 其他四指
                    is_extended = tip_y < pip_y
                
                current_finger_state[i] = is_extended
                if is_extended:
                    finger_count += 1
            
            # 检查手指状态是否有变化，如果有变化则控制舵机
            if current_finger_state != previous_finger_state:
                control_servos(current_finger_state)
                previous_finger_state = current_finger_state.copy()
            
            # 在图像上显示结果
            cv2.putText(frame, f"Fingers: {finger_count}", (10, 30), 
                       cv2.FONT_HERSHEY_SIMPLEX, 1, (0, 255, 0), 2)
            
            # 显示每个手指的状态
            for i, name in enumerate(finger_names):
                status = "Extended" if current_finger_state[i] else "Bent"
                color = (0, 255, 0) if current_finger_state[i] else (0, 0, 255)
                y_position = 70 + i * 25
                cv2.putText(frame, f"{name}: {status}", (10, y_position), 
                           cv2.FONT_HERSHEY_SIMPLEX, 0.6, color, 2)
    
    cv2.imshow('w', frame)
    if cv2.waitKey(1) & 0xFF == 27:  # 按ESC退出
        break

# 释放资源
cap.release()
cv2.destroyAllWindows()